#include "solve.h"
#include <math.h>

//Public Methods Implementation
Solve::Solve(DMatrix _m, DVector _v)
{
 m_dim = _v.getLength();
 if (_m.getCols()==m_dim)
 {
  m_m = _m;
  m_v = _v;
  DVector x(m_dim);
  x.zero();
  m_x0 = x;
  double max = fabs(m_m(0,0)), min = fabs(m_v(0)), test1, test2;
  for(unsigned int i = 1; i < m_m.getRows(); ++i)
  {
   test1 = fabs(m_m(i,i));
   test2 = fabs(m_v(i));
   if (test1>max)
   {
    max = test1;
   }
   if (test2<min)
   {
    min = test2;
   }
  }
  if (min == 0)
  {
   min = 1;
  }
  m_tol = min/max/1000;
  m_maxIter = m_dim + 2;
  m_type = 1;
  m_linType = 4;
  m_A.resize(m_dim,m_dim);
  m_A.zero();
  m_b.resize(m_dim);
  m_b.zero();
  m_dir.resize(m_dim);
  m_dir.zero();
 } 
 else
  cout<<"ERROR 001: Matrix and Vector's dimensions mismatched";
}
//-------------------------------------------------------------------------
Solve::Solve(DMatrix _m, DVector _v, DVector _x0)
{
 m_dim = _v.getLength();
 if (_m.getCols()==m_dim)
 {
  if (_x0.getLength()==m_dim)
  {
   m_m = _m;
   m_v = _v;
   m_x0 = _x0;
   double max = fabs(m_m(0,0)), min = fabs(m_v(0)), test1, test2;
   for(unsigned int i = 1; i < m_m.getRows(); ++i)
   {
    test1 = fabs(m_m(i,i));
    test2 = fabs(m_v(i));
    if (test1>max)
    {
     max = test1;
    }
    if (test2<min)
    {
     min = test2;
    }
   }
   if (min == 0)
   {
    min = 1;
   }
   m_tol = min/max/1000;
   m_maxIter = m_dim + 2;
   m_type = 1;
   m_linType = 4;
   m_A.resize(m_dim,m_dim);
   m_A.zero();
   m_b.resize(m_dim);
   m_b.zero();
   m_dir.resize(m_dim);
   m_dir.zero();
  } 
  else
   cout<<"ERROR 002: Vector's dimensions mismatched";
 } 
 else
  cout<<"ERROR 001: Matrix and Vector's dimensions mismatched";
}
//-------------------------------------------------------------------------
Solve::Solve(DMatrix _m, DVector _v, DVector _x0, double _tol)
{
 m_dim = _v.getLength();
 if (_m.getCols()==m_dim)
 {
  if (_x0.getLength()==m_dim)
  {
   if (_tol>0)
   {
    m_m = _m;
    m_v = _v;
    m_x0 = _x0;
    m_tol = _tol;
    m_maxIter = m_dim + 2;
    m_type = 1;
    m_linType = 4;
    m_A.resize(m_dim,m_dim);
    m_A.zero();
    m_b.resize(m_dim);
    m_b.zero();
    m_dir.resize(m_dim);
    m_dir.zero();
   } 
   else
    cout<<"ERROR 003: Tolerance must be a nonzero positive value";
  } 
  else
   cout<<"ERROR 002: Vector's dimensions mismatched";
 } 
 else
  cout<<"ERROR 001: Matrix and Vector's dimensions mismatched";
}
//-------------------------------------------------------------------------
Solve::Solve(DMatrix _m, DVector _v, DVector _x0, double _tol, int _maxIter)
{
 m_dim = _v.getLength();
 if (_m.getCols()==m_dim)
 {
  if (_x0.getLength()==m_dim)
  {
   if (_tol>0)
   {
    if ( _maxIter > 0 )
    {
     m_m = _m;
     m_v = _v;
     m_x0 = _x0;
     m_tol = _tol;
     m_maxIter = _maxIter;
     m_type = 1;
     m_linType = 4;
     m_A.resize(m_dim,m_dim);
     m_A.zero();
     m_b.resize(m_dim);
     m_b.zero();
     m_dir.resize(m_dim);
     m_dir.zero();
    }
    else
     cout<<"ERROR 004: Maximum iterator must be a nonzero natural value";				
   } 
   else
    cout<<"ERROR 003: Tolerance must be a nonzero positive value";
  } 
  else
   cout<<"ERROR 002: Vector's dimensions mismatched";
 } 
 else
  cout<<"ERROR 001: Matrix and Vector's dimensions mismatched";

}
//-------------------------------------------------------------------------
Solve::Solve(DMatrix _m, DVector _v, DVector _x0, double _tol, int _maxIter, int _type)
{
 m_dim = _v.getLength();
 if (_m.getCols()==m_dim)
 {
  if (_x0.getLength()==m_dim)
  {
   if (_tol>0)
   {
    if (  _maxIter > 0 )
    {
     if (_type > 0 )
     {
      m_m = _m;
      m_v = _v;
      m_x0 = _x0;
      m_tol = _tol;
      m_maxIter = _maxIter;
      m_type = _type;
      m_linType = 4;
      m_A.resize(m_dim,m_dim);
      m_A.zero();
      m_b.resize(m_dim);
      m_b.zero();
      m_dir.resize(m_dim);
      m_dir.zero();
     } 
     else
      cout<<"ERROR 005: Solve type must be a nonzero natural value";
    }
    else
     cout<<"ERROR 004: Maximum iterator must be a nonzero natural value";				
   } 
   else
    cout<<"ERROR 003: Tolerance must be a nonzero positive value";
  } 
  else
   cout<<"ERROR 002: Vector's dimensions mismatched";
 } 
 else
  cout<<"ERROR 001: Matrix and Vector's dimensions mismatched";

}
//-------------------------------------------------------------------------
Solve::Solve(DMatrix _m, DVector _v, DVector _x0, double _tol, int _maxIter, int _type, int _linType)
{
 m_dim = _v.getLength();
 if (_m.getCols()==m_dim)
 {
  if (_x0.getLength()==m_dim)
  {
   if (_tol>0)
   {
    if (  _maxIter > 0 )
    {
     if (_type > 0 )
     {
      if (_linType > 0 )
      {
       m_m = _m;
       m_v = _v;
       m_x0 = _x0;
       m_tol = _tol;
       m_maxIter = _maxIter;
       m_type = _type;
       m_linType = _linType;
       m_A.resize(m_dim,m_dim);
       m_A.zero();
       m_b.resize(m_dim);
       m_b.zero();
       m_dir.resize(m_dim);
       m_dir.zero();
      } 
      else
       cout<<"ERROR 005: Linear Search type must be a nonzero natural value";
     } 
     else
      cout<<"ERROR 005: Solve type must be a nonzero natural value";
    }
    else
     cout<<"ERROR 004: Maximum iterator must be a nonzero natural value";				
   } 
   else
    cout<<"ERROR 003: Tolerance must be a nonzero positive value";
  } 
  else
   cout<<"ERROR 002: Vector's dimensions mismatched";
 } 
 else
  cout<<"ERROR 001: Matrix and Vector's dimensions mismatched";
}
//-------------------------------------------------------------------------
Solve::~Solve()
{

}
//-------------------------------------------------------------------------
void 
Solve::changeMatrix(DMatrix _m)
{
 if(_m.getCols() == m_dim)
 {
  m_m = _m;
 }
}
//-------------------------------------------------------------------------
void 
Solve::changeVector(DVector _v)
{
 if(_v.getLength() == m_dim)
 {
  m_v = _v;
 }
}
//-------------------------------------------------------------------------
void
Solve::changeMatrixAndVector(DMatrix _m, DVector _v)
{
 int dim = _m.getCols();
 if(_v.getLength() == dim)
 {
  m_m = _m;
  m_v = _v;
  m_dim = dim;
 }

}
//-------------------------------------------------------------------------
void 
Solve::changeInitialPoint(DVector _x0)
{
 if(_x0.getLength() == m_dim)
 {
  m_x0 = _x0;
 }
}
//-------------------------------------------------------------------------
void 
Solve::changeTolerance(double _tol)
{
 m_tol = _tol;
}
//-------------------------------------------------------------------------
void 
Solve::changeMaxIteration(int _maxIter)
{
 m_maxIter = _maxIter;
}
//-------------------------------------------------------------------------
void 
Solve::changeSolveType(int _type)
{
 m_type = _type;
}
//-------------------------------------------------------------------------
void 
Solve::changeLinearSearchType(int _linType)
{
 m_linType = _linType;
}
//-------------------------------------------------------------------------
DVector 
Solve::execute()
{
 m_A = ~m_m*m_m;
 m_b = ~m_m*m_v;
 DVector X(m_dim);
 X.zero();
 switch (m_type)
 {
 case 1:
  {
   gradConj(X);
   break;
  }
 case 2:
  {
   fletcherReeves(X);
   break;
  }
 case 3:
  {
   BFGS(X);
   break;
  }
 }
 return X;
}

//Private Methods Implementation
// Solvers type:

void 
Solve::gradConj(DVector & _x)
{
 int i = 0;
 double stop;
 double alfa, a1, beta, bNew, bOld;
 double param, fractpart, intpart;
 DVector r(m_dim);
 DVector q(m_dim);
 r = m_b - m_A*m_x0;
 m_dir = r;
 _x = m_x0;
 bNew = r*r;
 stop = pow(m_tol,2)*bNew;
 while ( bNew>stop && i<m_maxIter )
 {
  q = m_A*m_dir;
  a1 = m_dir*q;
  alfa = bNew/a1;
  _x = _x + alfa*m_dir;
  param = i/50.0;
  fractpart = modf (param , &intpart);
  if ( fractpart == 0 )
  {
   r = m_b - m_A*_x;
  }
  else
  {
   r -= q*alfa;
  }
  bOld = bNew;
  bNew = r*r;
  beta = bNew/bOld;
  m_dir = r + beta*m_dir;
  i += 1;
 }
}
//-------------------------------------------------------------------------
void 
Solve::fletcherReeves(DVector & _x)
{
 int i = 0;
 DVector r(m_dim);
 _x = m_x0;
 r = m_b - m_A*m_x0;
 m_dir = r;
 double a, beta, b0, b1;
 b0 = r*r;
 double erro = pow(m_tol,2)*b0;
 while ( b0>erro && i<m_maxIter )
 {
  switch (m_linType)
  {
  case 1:
   {
    a = bisecao(-erro/2,erro/2);
    break;
   }
  case 2:
   {
    a = goldSection(0,erro);
    break;
   }
  case 3:
   {
    a = ortogonalidade();
    break;
   }
  case 4:
   {
    a = secante();
    break;
   }
  }
  _x = _x + a*m_dir;
  b1 = b0;
  r = m_b - m_A*(_x);
  b0 = r*r;
  beta = b0/b1;
  m_dir = m_dir*beta + r;
  i += 1;
 } 
}
//-------------------------------------------------------------------------
void 
Solve::BFGS(DVector & _x)
{
 double a;
 switch (m_linType)
 {
 case 1:
  {
   a = bisecao(0,1000*m_tol);
   break;
  }
 case 2:
  {
   a = goldSection(0,1000*m_tol);
   break;
  }
 case 3:
  {
   a = ortogonalidade();
   break;
  }
 case 4:
  {
   a = secante();
   break;
  }
 }
}
//-------------------------------------------------------------------------

// Linear Search type:
double 
Solve::bisecao(double _a, double _b)
{
 double pm;
 double erro;
 double sga = 0;
 double sgb = 0;
 double sgm = 0;
 DVector ga(m_dim);
 DVector gb(m_dim);
 DVector gm(m_dim);
 if (_a>_b)
 {
  erro = _b;
  _b = _a;
  _a = erro;
 }
 ga = m_A*(m_x0+m_dir*_a) - m_b;
 gb = m_A*(m_x0+m_dir*_b) - m_b;
 for (int i = 0; i<m_dim; ++i)
 {
  sga += m_dir(i)*ga(i);
  sgb += m_dir(i)*gb(i);
 }
 erro = fabs(sga)/sga;
 pm = fabs(sgb)/sgb;
 if (pm == erro)
 {
  if (sga<sgb)
   return _a;
  else
   return _b;
 } 
 else
 {
  sga = erro;
  sgb = pm;
 }
 do 
 {
  pm = (_a+_b)/2;
  gm = m_A*(m_x0+m_dir*pm) - m_b;
  sgm = gm*m_dir;
  sgm = fabs(sgm)/sgm;
  if (sga == sgm)
  {
   _a = pm;
   sga = sgm;
  }
  else
  {
   _b = pm;
   sgb = sgm;
  }
  erro = _b -_a;	

 } while (erro>m_tol);
 if (sgm<0)
  return pm; 
 else
  return _a;
}
//-------------------------------------------------------------------------
double 
Solve::goldSection(double _a, double _b)
{
 return 3.14;
}
//-------------------------------------------------------------------------
double 
Solve::ortogonalidade()
{
 double sga0 = 0;
 double sga = 0;
 double a = m_tol;
 DVector ga(m_dim);	ga = m_A*(m_x0) - m_b;
 sga0 = m_dir*ga;
 sga0 = fabs(sga0)/sga0;
 do 
 {
  sga = 0;
  ga = m_A*(m_x0+m_dir*a) - m_b;
  sga = m_dir*ga;
  sga = fabs(sga)/sga;
  a = a + m_tol;
 } while (sga==sga0);
 return a - 2*m_tol;
}
//-------------------------------------------------------------------------
double 
Solve::secante()
{
 double sga0 = 0;
 double sga = 0;
 double a = m_tol;
 double a0 = 0;
 DVector ga(m_dim);
 DVector ga0(m_dim);
 ga = m_A*(m_x0+m_dir*a) - m_b;
 ga0 = m_A*(m_x0+m_dir*a0) - m_b;
 for (int i = 0; i<m_dim; ++i)
 {
  sga += m_dir(i)*ga(i);
  sga0 += m_dir(i)*ga0(i);
 }
 do
 {
  a = (sga*a0-sga0*a)/(sga-sga0);
  sga = 0;
  sga0 = 0;
  ga = m_A*(m_x0+m_dir*a) - m_b;
  ga0 = m_A*(m_x0+m_dir*a0) - m_b;
  for (int i = 0; i<m_dim; ++i)
  {
   sga += m_dir(i)*ga(i);
   sga0 += m_dir(i)*ga0(i);
  }
  a0 = a;
 } while (fabs(sga)>m_tol);
 return a;
}
//-------------------------------------------------------------------------

void teste(){
}